<HTML> 
<HEAD> 
	<TITLE>3rd Party ROBOTC Drivers</TITLE> 
<link href="tabs.css" rel="stylesheet" type="text/css"/> 
<link href="doxygen.css" rel="stylesheet" type="text/css"/> 
</HEAD> 
 
<BODY bgcolor="#ffffff" link="#000000" vlink="#000000"> 
<table width="100%" bgcolor="navy" cellspacing=0 cellpadding=1 border=0> 
<tr><td><table width="100%" bgcolor="#EEEEDD" cellspacing=0 cellpadding=3 border=0> 
<tr> 
    <td width="33%" align="left"> <img src="images/LOGO_NXT.gif" width=266 height=44 border="0" alt="Mindstorms"></td> 
    <td><b><font size="+3" color="navy">3rd Party ROBOTC Drivers</font></b></td> 
    <td align="right"> <img src="images/logo.png" width=44 height=44 border="0" alt="RobotC"></td>     
</tr> 
  <tr bgcolor="#cccc99" align="right"> 
          <td colspan=3> <font face="arial" size="-1"> [<a href=main.html>Home</a>] [<a target=_top href="https://sourceforge.net/projects/rdpartyrobotcdr/">Download</a>] [<a target=_top href="http://apps.sourceforge.net/mantisbt/rdpartyrobotcdr/my_view_page.php">Submit a bug/suggestion</a>]  [<a target=_top href="http://www.robotc.net/forums/">ROBOTC Forums</a>] [<a target=_top href="http://botbench.com">Blog</a>] [<a target=_top href="http://sourceforge.net/donate/index.php?group_id=257238">Support this project</a>]</font> </td> 
</tr> 
</table></td></tr> 
</table> 
<!-- Generated by Doxygen 1.7.2 -->
<div class="header">
  <div class="headertitle">
<h1>mindsensors-lineleader-test3.c</h1>  </div>
</div>
<div class="contents">
<div class="fragment"><pre class="fragment"><span class="preprocessor">#pragma config(Sensor, S1,     LLEADER,             sensorI2CCustom9V)</span>
<span class="preprocessor"></span><span class="comment">//*!!Code automatically generated by &#39;ROBOTC&#39; configuration wizard               !!*//</span>

<span class="comment">/*</span>
<span class="comment"> * $Id: mindsensors-lineleader-test3.c 133 2013-03-10 15:15:38Z xander $</span>
<span class="comment"> */</span>
<span class="comment"></span>
<span class="comment">/** \file mindsensors-lineleader-test3.c</span>
<span class="comment"> * \brief Mindsensors LineLeader Sensor demo program</span>
<span class="comment"> *</span>
<span class="comment"> * mindsensors-lineleader-test3.c is a demo program for the Mindsensors LineLeader Sensor.</span>
<span class="comment"> *</span>
<span class="comment"> * Changelog:</span>
<span class="comment"> * - 0.1: Initial release</span>
<span class="comment"> * - 0.2: Reworked to use new driver API</span>
<span class="comment"> * - 0.3: More comments&lt;br&gt;</span>
<span class="comment"> *        Use clip() instead of manual clipping</span>
<span class="comment"> * - 0.4: Updated to use new array types that don&#39;t use structs\n</span>
<span class="comment"> *        Removed common.h from includes</span>
<span class="comment"> *</span>
<span class="comment"> * Credits:</span>
<span class="comment"> * - Big thanks to Mindsensors for providing me with the hardware necessary to write and test this.</span>
<span class="comment"> *</span>
<span class="comment"> * License: You may use this code as you wish, provided you give credit where its due.</span>
<span class="comment"> *</span>
<span class="comment"> * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. </span>
<span class="comment"></span>
<span class="comment"> * Xander Soldaat (xander_at_botbench.com)</span>
<span class="comment"> * 14 February 2011</span>
<span class="comment"> * version 0.4</span>
<span class="comment"> */</span>

<span class="preprocessor">#define MSLL_I2C_ADDR 0x02</span>
<span class="preprocessor"></span>
<span class="preprocessor">#include &quot;<a class="code" href="mindsensors-lineleader_8h.html" title="Mindsensors Line Tracking Sensor.">drivers/mindsensors-lineleader.h</a>&quot;</span>

<span class="preprocessor">#define printXY nxtDisplayStringAt</span>
<span class="preprocessor"></span><span class="preprocessor">#define println nxtDisplayTextLine</span>
<span class="preprocessor"></span><span class="preprocessor">#define clearln nxtDisplayClearTextLine</span>
<span class="preprocessor"></span>
<span class="comment">// The PID constants and base speed</span>
<span class="preprocessor">#define DEFAULT_KP    5</span>
<span class="preprocessor"></span><span class="preprocessor">#define DEFAULT_KI    0</span>
<span class="preprocessor"></span><span class="preprocessor">#define DEFAULT_KD   30</span>
<span class="preprocessor"></span><span class="preprocessor">#define DEFAULT_SP   50</span>
<span class="preprocessor"></span>
<span class="comment">// The file for the log data</span>
<span class="preprocessor">#define LOGFILE   &quot;linelead.dat&quot;</span>
<span class="preprocessor"></span>
<span class="preprocessor">#define MENUITEMS 5</span>
<span class="preprocessor"></span>
<span class="comment">// function prototypes</span>
<span class="keywordtype">void</span> doMainMenu();
<span class="keywordtype">void</span> doMenuItem(<span class="keywordtype">int</span> activeOption);
<span class="keywordtype">bool</span> checkTimer(TTimers timer);
<span class="keywordtype">void</span> doLineLead();
<span class="keywordtype">void</span> writeParams();

<span class="keywordtype">string</span> menuHeader;
<span class="keywordtype">string</span> menuFooter;

<span class="keyword">const</span> TTimers rightButtonTimer = T1;
<span class="keyword">const</span> TTimers leftButtonTimer  = T2;
<span class="keyword">const</span> TTimers enterButtonTimer = T3;

<span class="keywordtype">int</span> activeOption = 0;

<span class="keywordtype">int</span> keep_running = 0;

byte sensor = 0;

<span class="comment">// array holding default values for PID constants and base speed</span>
<span class="keyword">const</span> <span class="keywordtype">int</span> default_params[4] = {
                              DEFAULT_KP,
                              DEFAULT_KI,
                              DEFAULT_KD,
                              DEFAULT_SP };

<span class="comment">// array to hold user entered values</span>
<span class="keywordtype">int</span> params[4];

<span class="keyword">const</span> <span class="keywordtype">string</span> optionMainMenu[5] = {
                              <span class="stringliteral">&quot;Kp&quot;</span>,
                              <span class="stringliteral">&quot;Ki&quot;</span>,
                              <span class="stringliteral">&quot;Kd&quot;</span>,
                              <span class="stringliteral">&quot;Sp&quot;</span>,
                              <span class="stringliteral">&quot;Run&quot;</span> };

<span class="keyword">const</span> <span class="keywordtype">string</span> optionMainMenuFooter[5] = {
                              <span class="stringliteral">&quot;Ent=Edit&quot;</span>,
                              <span class="stringliteral">&quot;Ent=Edit&quot;</span>,
                              <span class="stringliteral">&quot;Ent=Edit&quot;</span>,
                              <span class="stringliteral">&quot;Ent=Edit&quot;</span>,
                              <span class="stringliteral">&quot;Ent=Run&quot;</span> };

<a name="a0"></a><a class="code" href="group__common.html#gaa172ef131853460e5b9f5c6db81d30f4">tByteArray</a> signalstr;


<span class="comment">// This function draws the current values of the sensor and other data on the screen</span>
<span class="comment">// in a visually pleasing way</span>
task drawSensors() {
  <span class="keywordflow">while</span> (keep_running == 1) {
    <span class="comment">// This clears the entire area occupied by the small rectangles</span>
    nxtEraseRect(6,62, 91, 43);
    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 8; i++) {
      <span class="comment">// Draw the rectangles for the signal strength first</span>
      nxtDrawRect(6+(i*11),62, 14+(i*11), 50);
      nxtFillRect(6+(i*11),51+signalstr[i]/10, 14+(i*11), 50);
      <span class="comment">// Draw a black rectangle if the sensor has detected the line,</span>
      <span class="comment">// a hollow one when nothing has been detected.</span>
      <span class="keywordflow">if</span> ((sensor &gt;&gt; i) &amp; 1) {
        nxtFillRect(6+(i*11),48, 14+(i*11), 43);
      } <span class="keywordflow">else</span> {
        nxtDrawRect(6+(i*11),48, 14+(i*11), 43);
      }
    }
    wait1Msec(100);
  }
}


<span class="comment">// Keep the robot on the line!</span>
task followTheYellowBrickRoad () {
  <span class="keywordtype">int</span> powerA = 0;
  <span class="keywordtype">int</span> powerC = 0;
  byte steering = 0;

  eraseDisplay();
  nxtDisplayCenteredTextLine(3, <span class="stringliteral">&quot;Running...&quot;</span>);
  nxtDisplayCenteredTextLine(5, <span class="stringliteral">&quot;Press exit&quot;</span>);
  nxtDisplayCenteredTextLine(6, <span class="stringliteral">&quot;to stop&quot;</span>);

  time1[T4] = 0;
  <span class="keywordflow">while</span> (keep_running == 1) {
    steering = <a name="a1"></a><a class="code" href="group__msll.html#ga5c46604095f77a86983b7aeaf22200dc">LLreadSteering</a>(LLEADER);
    sensor = <a name="a2"></a><a class="code" href="group__msll.html#ga7dd78212c25b63c0349b8f4c61e11e1b">LLreadResult</a>(LLEADER);
    <a name="a3"></a><a class="code" href="group__msll.html#ga60491a411576c00b70909e1f295919db">LLreadSensorRaw</a>(LLEADER, signalstr);

    powerA = (params[3] + steering);
    powerC = (params[3] - steering);

    <span class="comment">// If your robot is going in the wrong direction, comment out the</span>
    <span class="comment">// lines above and uncomment the lines below.</span>
    <span class="comment">//powerA = (params[3] - steering);</span>
    <span class="comment">//powerC = (params[3] + steering);</span>

    <span class="comment">// this clips the values</span>
    powerA = <a name="a4"></a><a class="code" href="group__common.html#ga4090c86dbda268a45068c4413fba327d">clip</a>(powerA, -100, 100);
    powerC = <a class="code" href="group__common.html#ga4090c86dbda268a45068c4413fba327d">clip</a>(powerC, -100, 100);

    <span class="comment">//if(powerA&gt;100)  powerA=100;</span>
    <span class="comment">//if(powerA&lt;-100) powerA=-100;</span>
    <span class="comment">//if(powerC&lt;-100) powerC=-100;</span>
    <span class="comment">//if(powerC&gt;100)  powerC=100;</span>

    motor[motorA] = (byte)powerA;
    motor[motorC] = (byte)powerC;
                wait1Msec(1);
                <span class="keywordflow">if</span> (sensor != 0xFF) {
                  time1[T4] = 0;
          } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (time1[T4] &gt; 500) {
            keep_running = 0;
          }
  }

  motor[motorA] = 0;
  motor[motorC] = 0;
}

task redrawMenu() {
  <span class="keywordflow">while</span>(<span class="keyword">true</span>) {
    eraseDisplay();
    println(0, menuHeader);
    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; MENUITEMS; i++) {
      clearln(i + 1);
      <span class="keywordflow">if</span> (i == activeOption) {
        <span class="keywordflow">if</span> (i &lt; 4)
          println(i + 1, <span class="stringliteral">&quot;&gt; %s [%3d] &lt;&quot;</span>, optionMainMenu[i], params[i]);
        <span class="keywordflow">else</span>
          println(i + 1, <span class="stringliteral">&quot;&gt; %s      &lt;&quot;</span>, optionMainMenu[i]);
        println(7, menuFooter);
      } <span class="keywordflow">else</span> {
        <span class="keywordflow">if</span> (i &lt; 4)
          println(i + 1, <span class="stringliteral">&quot;  %s [%3d]&quot;</span>, optionMainMenu[i], params[i]);
        <span class="keywordflow">else</span>
          println(i + 1, <span class="stringliteral">&quot;  %s&quot;</span>, optionMainMenu[i]);
      }
    }
    wait1Msec(100);
  }
}

<span class="comment">// Main task</span>
task main () {

  <span class="comment">// Set the default parameters and write them to the sensor</span>
  memcpy(params, default_params, <span class="keyword">sizeof</span>(default_params));
  writeParams();

  nNxtButtonTask  = -2;
  nNxtExitClicks = 3;

  StartTask(redrawMenu);
  doMainMenu();
  <span class="keywordflow">while</span>(<span class="keyword">true</span>)
    wait1Msec(100);
}

<span class="comment">// Draw the main menu</span>
<span class="keywordtype">void</span> doMainMenu () {
  <span class="keywordflow">while</span> (<span class="keyword">true</span>) {
    menuHeader = <span class="stringliteral">&quot;L/R to select&quot;</span>;
    menuFooter = optionMainMenuFooter[activeOption];

    <span class="keywordflow">switch</span>(nNxtButtonPressed) {
      <span class="keywordflow">case</span> kRightButton:
            <span class="keywordflow">if</span> (!checkTimer(rightButtonTimer)) {
              <span class="keywordflow">break</span>;
            }
            <span class="keywordflow">if</span> (activeOption == (MENUITEMS - 1))
              activeOption = 0;
            <span class="keywordflow">else</span>
              activeOption++;
            menuFooter = optionMainMenuFooter[activeOption];
            wait1Msec(300);
            <span class="keywordflow">break</span>;
      <span class="keywordflow">case</span> kLeftButton:
            <span class="keywordflow">if</span> (!checkTimer(leftButtonTimer)) {
              <span class="keywordflow">break</span>;
            }
            <span class="keywordflow">if</span> (activeOption == 0)
              activeOption = (MENUITEMS - 1);
            <span class="keywordflow">else</span>
              activeOption--;
            menuFooter = optionMainMenuFooter[activeOption];
            wait1Msec(300);
            <span class="keywordflow">break</span>;
      <span class="keywordflow">case</span> kEnterButton:
            <span class="keywordflow">if</span> (!checkTimer(rightButtonTimer)) {
              <span class="keywordflow">break</span>;
            }
            wait1Msec(600);
            doMenuItem(activeOption);
            <span class="keywordflow">break</span>;
      <span class="keywordflow">case</span> kExitButton:
            wait1Msec(500);
            StopAllTasks();

    }
  }
}

<span class="comment">// When enter is pressed in the doMainMenu() we come here.</span>
<span class="comment">// Here we handle the actual actions for each menu item.</span>
<span class="keywordtype">void</span> doMenuItem(<span class="keywordtype">int</span> activeOption) {
  PlaySound(soundBlip);
  <span class="keywordflow">while</span>(bSoundActive) EndTimeSlice();

  <span class="keywordflow">if</span> (activeOption == 4) {
    doLineLead();
    <span class="keywordflow">return</span>;
  }

  <span class="keywordflow">while</span> (<span class="keyword">true</span>) {
    menuHeader = <span class="stringliteral">&quot;L/R to edit val&quot;</span>;
    menuFooter = <span class="stringliteral">&quot;Ent=Save/Exit=Def&quot;</span>;
    <span class="keywordflow">switch</span>(nNxtButtonPressed) {
      <span class="keywordflow">case</span> kRightButton:
            <span class="keywordflow">if</span> (!checkTimer(rightButtonTimer)) {
              <span class="keywordflow">break</span>;
            }
            <span class="keywordflow">if</span> (params[activeOption] &lt; 128)
              params[activeOption]++;
            <span class="keywordflow">break</span>;
      <span class="keywordflow">case</span> kLeftButton:
            <span class="keywordflow">if</span> (!checkTimer(leftButtonTimer)) {
              <span class="keywordflow">break</span>;
            }
            <span class="keywordflow">if</span> (params[activeOption] &gt; 0)
              params[activeOption]--;
            <span class="keywordflow">break</span>;
      <span class="keywordflow">case</span> kEnterButton:
            <span class="keywordflow">if</span> (!checkTimer(enterButtonTimer)) {
              <span class="keywordflow">break</span>;
            }
            writeParams();
            wait1Msec(600);
            <span class="keywordflow">return</span>;
            <span class="keywordflow">break</span>;
      <span class="keywordflow">case</span> kExitButton:
            params[activeOption] = default_params[activeOption];
            writeParams();
            wait1Msec(600);
            <span class="keywordflow">break</span>;
    }
  }
}

<span class="comment">// See if more than 300ms has elapsed</span>
<span class="keywordtype">bool</span> checkTimer(TTimers timer) {
  <span class="keywordflow">if</span> (time1[timer] &lt; 300) {
    <span class="keywordflow">return</span> <span class="keyword">false</span>;
  } <span class="keywordflow">else</span> {
    time1[timer] = 0;
    <span class="keywordflow">return</span> <span class="keyword">true</span>;
  }
}

<span class="comment">// Start and stop the line following task (followTheYellowBrickRoad)</span>
<span class="keywordtype">void</span> doLineLead() {
  sensor = 0;
  StopTask(redrawMenu);
  keep_running = 1;
  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 5; i++) {
    PlaySound(soundBlip);
    wait1Msec(600);
  }
  PlaySound(soundFastUpwardTones);
  <span class="keywordflow">while</span>(bSoundActive) EndTimeSlice();
  StartTask(drawSensors);
  StartTask(followTheYellowBrickRoad);
  <span class="keywordflow">while</span>(nNxtButtonPressed != kExitButton &amp;&amp; keep_running != 0) {
    wait1Msec(10);
  }
  <span class="comment">// this will kill off the followTheYellowBrickRoad task</span>
  keep_running = 0;
  wait1Msec(1000);
  StartTask(redrawMenu);
}

<span class="comment">// Write the PID values to the LineLeader sensor</span>
<span class="keywordtype">void</span> writeParams() {
  <a name="a5"></a><a class="code" href="group__msll.html#ga023144b86129e6828fa94b757016065f">LLsetKp</a>(LLEADER, params[0], 32);
  <a name="a6"></a><a class="code" href="group__msll.html#ga9ca02184d0ee468b2fdd7e537654d7d3">LLsetKi</a>(LLEADER, params[1], 32);
  <a name="a7"></a><a class="code" href="group__msll.html#ga5e59a3463a3e8b2665e6b3555da8f93d">LLsetKd</a>(LLEADER, params[2], 32);
}

<span class="comment">/*</span>
<span class="comment"> * $Id: mindsensors-lineleader-test3.c 133 2013-03-10 15:15:38Z xander $</span>
<span class="comment"> */</span>
</pre></div> </div>
</div>
<hr class="footer"/><address class="footer"><small>Generated on Wed Mar 13 2013 19:56:42 for ROBOTC Drivers by&#160;
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.2 </small></address>
</body>
</html>
